#ifndef _ROS_YORK_MSG_H
#define _ROS_YORK_MSG_H
#include <serial/serial.h>
#include "ros/ros.h"
#include <stdio.h>
#include <stdlib.h>
#include <geometry_msgs/Twist.h>      //发布cmd_vel话题需要的头文件
#include <nav_msgs/Odometry.h>        //发布里程计需要的头文件
#include <tf/transform_broadcaster.h> //发布tf坐标关系需要的头文件
#include <sensor_msgs/Imu.h>          //发布imu话题需要的数据类型
#include "ros_mec_msg/york.h"
#include "ros_mec_msg/motorv.h"
using namespace std;
//extern auto_driving::imu imu_sensor; // 定义imu传感器的数据
// #define HEADER 0XAB   // 帧尾
// #define ENDFRAME 0XBA // 帧头
#define Header 0xAE
#define Headerframe 0xEA
#define Ender 0xDF
#define Enderframe 0xFD

/***********************
 * 
 * version:12-11
 * 
 * author:yqy
 * 
 * 
 * project:york_msg
 * ****************** */



#define PI 3.1415926f            // PI //圆周率



union TG_CarV
{
    unsigned char data[42]; // 最后一位存储的是校验位置
    struct
    {
        uint8_t header;
        uint8_t headerframe;
        uint16_t len;
        float Motor_Va;
        float Motor_Vb;
        float Motor_Vc;
        float Motor_Vd;
        float Vx;
        float Vy;
        float Vz;
        float Power_Battery;
        uint8_t ender;
        uint8_t enderframe;
        uint16_t error;
        uint8_t stop_flag;
        uint8_t check_num; //43
        /* data */
    } Car_Data;
};

// ros向下位机发送数据的共用体嵌套结构体
union ROS_SEND_DATA
{
    unsigned char data[12];
    struct
    {
        uint8_t HEADER;      // 数据帧头1
        uint8_t HEADERFRAME; // 数据帧头2
        int16_t x_speed;  
        int16_t y_speed;
        int16_t z_speed;
        uint8_t stop_flag;  // 停止位
        uint8_t ENDER;      // 数据帧尾
        uint8_t ENDERFRAME; // 数据帧尾2
        uint8_t checknum;   // 校验位
    } robot_send_data;
};

/*用户可以通过这个共用体填充数据四个电机的数据，从而改变电机的速度 */
union ROS_Motor_A
{
    unsigned char data[16];
    struct
    {
        uint8_t HeaderA;
        uint8_t HeaderAframe;
        uint16_t start_flag;
        int16_t MotorA_v;
        int16_t MotorB_v;
        int16_t MotorC_v;
        int16_t MotorD_v;
        uint8_t stop_flag;  // 停止位
        uint8_t ENDER;      // 数据帧尾
        uint8_t ENDERFRAME; // 数据帧尾2
        uint8_t checknum;   // 校验位
    }TG_Motor;
};
/*建立统一的管理的类*/
// 注：写函数的时候，要从构造函数开始写，构造函数写函数的声明和赋值
class TG_york_car
{
public:
    TG_york_car();                     // 构造函数
    ~TG_york_car();                    // 析构函数
    void TG_auto_control(void);       // 定义控制函数，循环控制代码
    serial::Serial Usart_stm32serial; // 声明串口对象
private:
    ros::NodeHandle Nh_TG;      // 建立节点句柄
    ros::Publisher york_msg_pub; // 创建里程计和imu发布者
    ros::Subscriber Cmd_vel_sub,motor_vel_sub;   //订阅速度话题
    unsigned char Check_Sum(unsigned char Count_Number, unsigned char mode); // BBC校验函数
    void Cmdvel_callback(const geometry_msgs::Twist::ConstPtr &TGcmd_vel);    //接收导航的速度话题
    void Motorvel_callback(const ros_mec_msg::motorv::ConstPtr &TGmotor_vel); //用户控制电机的速度话题，m/s
    bool TG_Getserial_Data(void);                                            // 定义接受串口数据的函数
    bool TG_Mecmsg_Pub(void);                                                    //发布三轴速度，四个电机的速度m/s
    int TG_serial_baud_rate;                                                       // 定义串口的波特率
    ROS_SEND_DATA TG_Send_data;                                                    //  机器人发送数据
    string TG_usart_port_name;                                                    // 定义相关变量
    TG_CarV TG_Rece_car;                                                          //接受stm32的数据共用体
    ROS_Motor_A Send_MotorV;                                                      //发送四个电机速度的共用体
};

#endif
